//#include <NewSoftSerial.h>
#include <LiquidCrystal_SR.h>
#include <SD.h>

#include <TinyGPS.h>

/* This sample code demonstrates the normal use of a TinyGPS object.
   It requires the use of SoftwareSerial, and assumes that you have a
   4800-baud serial GPS device hooked up on pins 3(rx) and 4(tx).
*/

TinyGPS gps;
//NewSoftSerial nss(2,3);
LiquidCrystal_SR srlcd(9, 8, TWO_WIRE);

File myFile;
boolean sderror = false;
//String logmsg,oldmsg;
char logname[13];
boolean writelog = true;
boolean refresh = false;
boolean finish_init = false;

byte satnum=0;
float flat, flon, spd, alt, oflat, oflon;
unsigned long age;
int year;
byte month, day, hour, minute, second, hundredths;
String crs;
char sz[20];


static void gpsdump(TinyGPS &gps);
static bool feedgps();
static void print_date(TinyGPS &gps);
static void print_satnum(TinyGPS &gps);
static void print_pos(TinyGPS &gps);
static void print_alt(TinyGPS &gps);
static void print_speed(TinyGPS &gps);
static void print_course(TinyGPS &gps);
static String float2str(float val, int len);
String int2str(int val,byte len,byte pad,char c);

String progress="   ";
int count;



ISR(TIMER1_OVF_vect) {
  TCNT1=0x0BDC; // set initial value to remove time error (16bit counter register)
  if (finish_init) refresh = true;
}


void setup()
{

  finish_init = false;
  
  TIMSK1=0x01; // enabled global and timer overflow interrupt;
  TCCR1A = 0x00; // normal operation page 148 (mode0);
  TCNT1=0x0BDC; // set initial value to remove time error (16bit counter register)
  TCCR1B = 0x04; // start timer/ set clock

  pinMode(7, OUTPUT);
  pinMode(10, OUTPUT);
  //oldmsg="";
  
  oflat = 0;
  oflon = 0;
  logname[0]=' ';
  
  Serial.begin(4800);
  //nss.begin(4800);

  delay(50);
  srlcd.begin(16,2);               // initialize the lcd
  
  uint8_t sat[8]  = {
    B00000000,
    B00010001,
    B00001001,
    B00000010,
    B00000100,
    B00011010,
    B00000111};
  uint8_t kmph[8]  = {
    B00010010,
    B00011100,
    B00010011,
    B00000000,
    B00011011,
    B00010101,
    B00010101};
  uint8_t sdw[8] = {
    B00000000,
    B00001111,
    B00011111,
    B00011111,
    B00011111,
    B00010001,
    B00011111};
  uint8_t sde[8] = {
    B00000000,
    B00001111,
    B00011111,
    B00010101,
    B00011011,
    B00010101,
    B00011111};
    
  srlcd.createChar(0, sat);
  srlcd.createChar(1, kmph);
  srlcd.createChar(2, sdw);
  srlcd.createChar(3, sde);
  delay(20);
  srlcd.home();
  delay(20);
  srlcd.setCursor(13,0);
  srlcd.print(char(0));
  delay(10);
  digitalWrite(7,HIGH);

  //chipSelect = 10
  if (!SD.begin(10)) {
    srlcd.setCursor(0,1);
    srlcd.print(F("SD init error!"));
    srlcd.setCursor(12,0);
    srlcd.print(char(3));
    sderror = true;
    //sd.initErrorHalt();
  }
  
  //Serial.println(sderror,DEC);
  /*Serial.print("Testing TinyGPS library v. "); Serial.println(TinyGPS::library_version());
  Serial.println("by Mikal Hart");
  Serial.println();
  Serial.print("Sizeof(gpsobject) = "); Serial.println(sizeof(TinyGPS));
  Serial.println();
  Serial.println("Sats HDOP Latitude Longitude Fix  Date       Time       Date Alt     Course Speed Card  Distance Course Card  Chars Sentences Checksum");
  Serial.println("          (deg)    (deg)     Age                        Age  (m)     --- from GPS ----  ---- to London  ----  RX    RX        Fail");
  Serial.println("--------------------------------------------------------------------------------------------------------------------------------------");
  */
  
  finish_init = true;
}

void loop()
{
   feedgps();
   if (refresh)
   {
      count %=15;
      
      if (satnum == TinyGPS::GPS_INVALID_SATELLITES)
      {
        for (int i=0;i<3;i++)
          if (i==count % 3) progress[i]='>';
            else progress[i]=' ';
      }

      
      writelog = true;
    
      gpsdump(gps);
    
      if (count % 5 == 0 && writelog)
      {
        logEvent();
      }

      refresh = false;
      count++;
   }
 
}

static void gpsdump(TinyGPS &gps)
{
  //float flat, flon;
  //unsigned long age, date, time, chars = 0;
  //unsigned short sentences = 0, failed = 0;
  //static const float LONDON_LAT = 51.508131, LONDON_LON = -0.128002;
  

  print_satnum(gps);
  print_date(gps);
  print_pos(gps);
  print_speed(gps);
  print_alt(gps);
  print_course(gps);
  
  //print_int(gps.satellites(), TinyGPS::GPS_INVALID_SATELLITES, 5);
  //print_int(gps.hdop(), TinyGPS::GPS_INVALID_HDOP, 5);
  //print_float(flat, TinyGPS::GPS_INVALID_F_ANGLE, 9, 5);
  //print_float(flon, TinyGPS::GPS_INVALID_F_ANGLE, 10, 5);
  //print_int(age, TinyGPS::GPS_INVALID_AGE, 5);
  //print_float(gps.f_altitude(), TinyGPS::GPS_INVALID_F_ALTITUDE, 8, 2);
  //print_float(gps.f_course(), TinyGPS::GPS_INVALID_F_ANGLE, 7, 2);
  //print_float(gps.f_speed_kmph(), TinyGPS::GPS_INVALID_F_SPEED, 6, 2);
  //print_str(gps.f_course() == TinyGPS::GPS_INVALID_F_ANGLE ? "*** " : TinyGPS::cardinal(gps.f_course()), 6);
  //print_int(flat == TinyGPS::GPS_INVALID_F_ANGLE ? 0UL : (unsigned long)TinyGPS::distance_between(flat, flon, LONDON_LAT, LONDON_LON) / 1000, 0xFFFFFFFF, 9);
  //print_float(flat == TinyGPS::GPS_INVALID_F_ANGLE ? 0.0 : TinyGPS::course_to(flat, flon, 51.508131, -0.128002), TinyGPS::GPS_INVALID_F_ANGLE, 7, 2);
  //print_str(flat == TinyGPS::GPS_INVALID_F_ANGLE ? "*** " : TinyGPS::cardinal(TinyGPS::course_to(flat, flon, LONDON_LAT, LONDON_LON)), 6);
  //gps.stats(&chars, &sentences, &failed);
  //print_int(chars, 0xFFFFFFFF, 6);
  //print_int(sentences, 0xFFFFFFFF, 10);
  //print_int(failed, 0xFFFFFFFF, 9);
  //Serial.println();
}

static void print_satnum(TinyGPS &gps)
{
  satnum = gps.satellites();
  if ( satnum != TinyGPS::GPS_INVALID_SATELLITES){
    //digitalWrite(7,HIGH);
    srlcd.setCursor(0,0);
    srlcd.print(F("            "));
    srlcd.setCursor(14,0);
    srlcd.print(int2str(satnum,2,1,'0'));
    writelog &= true;
  }
  else {
    //digitalWrite(7,LOW);
    srlcd.setCursor(0,0);
    srlcd.print(F("Searching"));
    srlcd.print(progress);
    srlcd.setCursor(14,0);
    srlcd.print(F("--"));
    writelog = false;
  }
  feedgps();
}

static void print_pos(TinyGPS &gps)
{
  gps.f_get_position(&flat, &flon, &age);
  
  if (flat != TinyGPS::GPS_INVALID_F_ANGLE && flon !=  TinyGPS::GPS_INVALID_F_ANGLE) {
    srlcd.setCursor(0,1);
    srlcd.print(float2str(flat,7));
    srlcd.print(F(" "));
    srlcd.print(float2str(flon,8));
    writelog &= true;
  }
  else
    writelog = false;
  
  feedgps();
}

static void print_alt(TinyGPS &gps)
{
  alt = gps.f_altitude();
  
  if (alt != TinyGPS::GPS_INVALID_F_ALTITUDE){
    writelog &= true;
  }
  else
  {
    writelog = false;
  }
  feedgps();
}

static void print_course(TinyGPS &gps)
{
    crs =(gps.f_course() == TinyGPS::GPS_INVALID_F_ANGLE) ? "*** " : TinyGPS::cardinal(gps.f_course());
}

static void print_date(TinyGPS &gps)
{
  gps.crack_datetime(&year, &month, &day, &hour, &minute, &second, &hundredths, &age);
  if (age != TinyGPS::GPS_INVALID_AGE && month>0 && day>0)
  {
    srlcd.setCursor(7,0);
    srlcd.print(int2str(((hour+8) % 24),2,1,'0')+(second % 2 ? ":" : " ")+int2str(minute,2,1,'0'));
    writelog &= true;
  }
  else
    writelog = false;
  feedgps();
}

static void print_speed(TinyGPS &gps)
{
  spd = gps.f_speed_kmph();
  if (spd != TinyGPS::GPS_INVALID_F_SPEED) 
  {
    srlcd.setCursor(0,0);
    srlcd.print(float2str(spd,5));
    srlcd.print(char(1));
    //srlcd.print("km/h");
    writelog &= true;
  }
  else 
    writelog = false;
  feedgps();
}


static bool feedgps()
{
  while (Serial.available())
  {
    if (gps.encode(Serial.read()))
      return true;
  }
  return false;
}

static String float2str(float val, int len)
{
  String str = "";
  int pos = 0;
  int p1;
  bool minus=false;
  
  if (val<0)
  {
    minus=true;
    len--;
    val = abs(val);
  }
  
  p1=(int)val;
  val=val-p1;
  
  if (p1==0) str="0";
  
  for (pos=0;pos<len && p1>0;pos++)
  {
    str= ((char)('0'+(p1 % 10))) + str;
    p1=p1/10;
  }
  if (pos<len && val>0){
    pos++;
    str += ".";
  }
  for (;pos<len&& val>0;pos++)
  {
    str += (char)('0'+ (int)(val*10));
    val= val * 10 - ((int)(val*10));
  }
  
  if (minus) str = "-" + str;  
  return str;
  
}

static String int2str(int val,byte len,byte pad,char c)
{
  String str;
  byte pos = 0;
  
  for (pos;pos<len && val>0;pos++){
    str= ((char)('0'+val % 10)) + str; 
    val= val /10;   
  }
  
  if (pos<len) {
   switch(pad){
     case 1: for (;pos<len;pos++) str = (char)c + str; break;
     case 2: for (;pos<len;pos++) str = str + (char)c; break;
   }
  }
  
  return str;
}

void logEvent()
{

  //logmsg ="";
  sprintf(sz, "%04d-%02d-%02d,%02d:%02d:%02d,",  year, month, day, hour, minute, second);
  //logmsg.concat(sz);
  if (logname[0]==' ')
    sprintf(logname, "%04d%02d%02d.trc",  year, month, day, hour, minute, second);
  //logmsg += int2str(satnum,2,0,' ')+",";    
  //logmsg += float2str(flat,20)+","+float2str(flon,20)+",";
  //logmsg += float2str(alt,10)+",";
  //logmsg += float2str(spd,10)+",";
  //logmsg += crs+",";

  if (writelog && logname[0]!=' ' &&  (abs(flat-oflat) > 0.0001 || abs(flon - oflon) > 0.0001))
  {
    myFile=SD.open(logname, FILE_WRITE);
    if (!myFile) {
      srlcd.setCursor(0,1);
      //srlcd.print(F("SD Write error"));
      srlcd.print(logname);
      sderror = true;
      //SD error icon
      srlcd.setCursor(12,0);
      srlcd.print(char(3));
      //sd.errorHalt("opening test.txt for write failed");
    }
    else
    {
      //SD write icon
      srlcd.setCursor(12,0);
      srlcd.print(char(2));

      oflat = flat;
      oflon = flon;
      //Serial.print(sderror,DEC);
      myFile.print(sz);
      myFile.print(int2str(satnum,2,0,' '));
      myFile.print(",");
      myFile.print(float2str(flat,20));
      myFile.print(",");
      myFile.print(float2str(flon,20));
      myFile.print(",");
      myFile.print(float2str(alt,10));
      myFile.print(",");
      myFile.print(float2str(spd,10));
      myFile.print(",");
      myFile.print(crs);
      myFile.println(",");
      //myFile.println(logmsg);
      delay(20);
      myFile.flush();
      delay(50);
      myFile.close();
      
      //SD Write complete...
      srlcd.setCursor(12,0);
      srlcd.print(F(" "));
    }
    //oldmsg = logmsg;
  }
}
